load("//tools/lint:lint.bzl", "add_lint_tests")
load(
    "//tools/skylark:drake_cc.bzl",
    "drake_cc_googletest",
    "drake_cc_library",
    "drake_cc_package_library",
)
load(
    "//tools/skylark:test_tags.bzl",
    "mosek_test_tags",
)

package(default_visibility = ["//visibility:public"])

drake_cc_package_library(
    name = "optimization",
    visibility = ["//visibility:public"],
    deps = [
        ":c_iris_collision_geometry",
        ":convex_set",
        ":cspace_free_box",
        ":cspace_free_internal",
        ":cspace_free_polytope",
        ":cspace_free_polytope_base",
        ":cspace_free_structs",
        ":cspace_separating_plane",
        ":geodesic_convexity",
        ":graph_of_convex_sets",
        ":implicit_graph_of_convex_sets",
        ":iris",
        ":iris_internal",
    ],
)

drake_cc_library(
    name = "geodesic_convexity",
    srcs = ["geodesic_convexity.cc"],
    hdrs = ["geodesic_convexity.h"],
    deps = [
        "//solvers:mathematical_program",
        "//solvers:mathematical_program_result",
    ],
    implementation_deps = [
        "//geometry/optimization:convex_set",
        "//solvers:solve",
        "@common_robotics_utilities_internal//:common_robotics_utilities",
    ],
)

drake_cc_library(
    name = "convex_set",
    srcs = [
        "affine_ball.cc",
        "affine_subspace.cc",
        "cartesian_product.cc",
        "convex_hull.cc",
        "convex_set.cc",
        "hpolyhedron.cc",
        "hyperellipsoid.cc",
        "hyperrectangle.cc",
        "intersection.cc",
        "minkowski_sum.cc",
        "point.cc",
        "spectrahedron.cc",
        "vpolytope.cc",
    ],
    hdrs = [
        "affine_ball.h",
        "affine_subspace.h",
        "cartesian_product.h",
        "convex_hull.h",
        "convex_set.h",
        "hpolyhedron.h",
        "hyperellipsoid.h",
        "hyperrectangle.h",
        "intersection.h",
        "minkowski_sum.h",
        "point.h",
        "spectrahedron.h",
        "vpolytope.h",
    ],
    deps = [
        "//geometry:scene_graph",
        "//math:matrix_util",
        "//solvers:mathematical_program",
        "//solvers:mathematical_program_result",
    ],
    implementation_deps = [
        "//geometry:read_obj",
        "//solvers:aggregate_costs_constraints",
        "//solvers:choose_best_solver",
        "//solvers:clarabel_solver",
        "//solvers:get_program_type",
        "//solvers:gurobi_solver",
        "//solvers:ipopt_solver",
        "//solvers:mosek_solver",
        "//solvers:scs_solver",
        "//solvers:solve",
        "@common_robotics_utilities_internal//:common_robotics_utilities",
        "@qhull_internal//:qhull",
    ],
)

drake_cc_library(
    name = "c_iris_collision_geometry",
    srcs = ["c_iris_collision_geometry.cc"],
    hdrs = ["c_iris_collision_geometry.h"],
    deps = [
        "//geometry:geometry_ids",
        "//geometry:shape_specification",
        "//geometry/optimization:convex_set",
        "//multibody/rational:rational_forward_kinematics",
        "//multibody/tree:multibody_tree_indexes",
    ],
)

drake_cc_library(
    name = "cspace_separating_plane",
    srcs = ["cspace_separating_plane.cc"],
    hdrs = ["cspace_separating_plane.h"],
    deps = [
        "//common/symbolic:polynomial",
        "//geometry/optimization:c_iris_collision_geometry",
    ],
)

drake_cc_library(
    name = "cspace_free_box",
    srcs = ["cspace_free_box.cc"],
    hdrs = ["cspace_free_box.h"],
    deps = [
        ":cspace_free_polytope_base",
    ],
)

drake_cc_library(
    name = "cspace_free_polytope",
    srcs = [
        "cspace_free_polytope.cc",
    ],
    hdrs = ["cspace_free_polytope.h"],
    deps = [
        ":cspace_free_polytope_base",
    ],
)

drake_cc_library(
    name = "cspace_free_polytope_base",
    srcs = ["cspace_free_polytope_base.cc"],
    hdrs = ["cspace_free_polytope_base.h"],
    deps = [
        "//common/symbolic:monomial_util",
        "//geometry/optimization:c_iris_collision_geometry",
        "//geometry/optimization:cspace_free_internal",
        "//geometry/optimization:cspace_free_structs",
        "//geometry/optimization:cspace_separating_plane",
        "//solvers:choose_best_solver",
        "//solvers:mathematical_program",
        "//solvers:mosek_solver",
        "//solvers:solve",
    ],
)

drake_cc_library(
    name = "cspace_free_internal",
    srcs = [
        "cspace_free_internal.cc",
    ],
    hdrs = ["cspace_free_internal.h"],
    deps = [
        "//geometry/optimization:c_iris_collision_geometry",
        "//geometry/optimization:cspace_free_structs",
        "//geometry/optimization:cspace_separating_plane",
        "//solvers:choose_best_solver",
        "//solvers:mathematical_program",
    ],
)

drake_cc_library(
    name = "cspace_free_structs",
    srcs = ["cspace_free_structs.cc"],
    hdrs = ["cspace_free_structs.h"],
    deps = [
        ":c_iris_collision_geometry",
        "//common:parallelism",
        "//common/symbolic:rational_function",
        "//solvers:mathematical_program",
        "//solvers:mathematical_program_result",
        "//solvers:mosek_solver",
    ],
)

drake_cc_library(
    name = "graph_of_convex_sets",
    srcs = ["graph_of_convex_sets.cc"],
    hdrs = ["graph_of_convex_sets.h"],
    deps = [
        ":convex_set",
        "//common:parallelism",
        "//common/symbolic:expression",
        "//solvers:choose_best_solver",
        "//solvers:create_cost",
        "//solvers:get_program_type",
        "//solvers:mathematical_program_result",
        "//solvers:solver_interface",
    ],
    implementation_deps = [
        "//solvers:mosek_solver",
        "//solvers:solve",
    ],
)

drake_cc_library(
    name = "implicit_graph_of_convex_sets",
    srcs = ["implicit_graph_of_convex_sets.cc"],
    hdrs = ["implicit_graph_of_convex_sets.h"],
    deps = [
        ":graph_of_convex_sets",
    ],
)

drake_cc_library(
    name = "iris_internal",
    srcs = ["iris_internal.cc"],
    hdrs = ["iris_internal.h"],
    deps = [
        ":convex_set",
        "//multibody/plant",
        "//solvers:mathematical_program",
        "//solvers:solve",
    ],
)

drake_cc_library(
    name = "iris",
    srcs = ["iris.cc"],
    hdrs = ["iris.h"],
    deps = [
        ":convex_set",
        ":iris_internal",
        "//common:name_value",
        "//geometry:meshcat",
        "//geometry:scene_graph",
        "//multibody/plant",
        "//solvers:choose_best_solver",
        "//solvers:ipopt_solver",
        "//solvers:snopt_solver",
    ],
)

drake_cc_library(
    name = "test_utilities",
    testonly = 1,
    srcs = ["test_utilities.cc"],
    hdrs = ["test_utilities.h"],
    visibility = ["//visibility:private"],
    deps = [
        ":convex_set",
        "//geometry:scene_graph",
        "//solvers:mathematical_program",
        "//solvers:solve",
        "@googletest//:gtest_for_library",
    ],
)

drake_cc_googletest(
    name = "affine_ball_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//solvers:gurobi_solver",
        "//solvers:mosek_solver",
    ],
)

drake_cc_googletest(
    name = "affine_subspace_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/yaml",
    ],
)

drake_cc_googletest(
    name = "cartesian_product_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "convex_hull_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "convex_set_test",
    deps = [
        ":convex_set",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "convex_set_limit_malloc_test",
    deps = [
        ":convex_set",
        "//common/test_utilities:limit_malloc",
    ],
)

drake_cc_googletest(
    name = "cspace_free_polytope_base_test",
    deps = [
        ":c_iris_test_utilities",
        ":cspace_free_polytope_base",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:symbolic_test_util",
        "//geometry/optimization:cspace_free_internal",
    ],
)

drake_cc_googletest(
    name = "cspace_free_box_test",
    deps = [
        ":c_iris_test_utilities",
        ":cspace_free_box",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//common/test_utilities:symbolic_test_util",
        "//geometry/optimization:cspace_free_internal",
    ],
)

drake_cc_googletest(
    name = "cspace_free_box_with_mosek_test",
    timeout = "long",
    # Some of these test cases take a very long time to run. Run each test case
    # in its own process (in parallel) to improve test latency.
    shard_count = 12,
    tags = mosek_test_tags() + [
        # This test launches 2 threads to test both serial and parallel code
        # paths in CspaceFreePolytope.
        "cpu:2",
        # The memcheck build was disabled by accident:
        #  https://github.com/RobotLocomotion/drake/pull/18621
        # TODO(hongkai-dai) Figure out whether we can re-enable this.
        "no_memcheck",
    ],
    use_default_main = False,
    deps = [
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//common/test_utilities:symbolic_test_util",
        "//geometry/optimization:c_iris_test_utilities",
        "//geometry/optimization:cspace_free_box",
        "//multibody/inverse_kinematics",
        "//solvers:mosek_solver",
    ],
)

drake_cc_googletest(
    name = "cspace_free_polytope_test",
    deps = [
        ":c_iris_test_utilities",
        ":cspace_free_polytope",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:symbolic_test_util",
        "//geometry/optimization:cspace_free_internal",
    ],
)

drake_cc_googletest(
    name = "cspace_free_internal_test",
    deps = [
        ":c_iris_test_utilities",
        ":cspace_free_internal",
    ],
)

drake_cc_googletest(
    name = "cspace_free_polytope_with_mosek_test",
    timeout = "long",
    # Some of these test cases take a very long time to run. Run each test case
    # in its own process (in parallel) to improve test latency.
    shard_count = 12,
    tags = mosek_test_tags() + [
        # This test launches 2 threads to test both serial and parallel code
        # paths in CspaceFreePolytope.
        "cpu:2",
        # This test experiences random segfaults in the "LSan Everything" build
        # when both Mosek and Gurobi are used at the same time. Since we can't
        # debug closed-source software, we have no choice but to ignore those
        # errors.
        "no_lsan",
        # The memcheck build was disabled by accident:
        #  https://github.com/RobotLocomotion/drake/pull/18621
        # TODO(hongkai-dai) Figure out whether we can re-enable this.
        "no_memcheck",
    ],
    use_default_main = False,
    deps = [
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//common/test_utilities:symbolic_test_util",
        "//geometry/optimization:c_iris_test_utilities",
        "//geometry/optimization:cspace_free_polytope",
        "//solvers:mosek_solver",
        "//solvers:sos_basis_generator",
    ],
)

drake_cc_library(
    name = "c_iris_test_utilities",
    testonly = 1,
    srcs = ["test/c_iris_test_utilities.cc"],
    hdrs = ["test/c_iris_test_utilities.h"],
    data = [
        "test/convex.mtl",
        "test/convex.obj",
    ],
    deps = [
        ":cspace_free_box",
        ":cspace_free_polytope",
        "//geometry:scene_graph",
        "//multibody/plant",
        "//solvers:sos_basis_generator",
        "//systems/framework:diagram_builder",
        "@googletest//:gtest_for_library",
    ],
)

drake_cc_googletest(
    name = "c_iris_collision_geometry_test",
    tags = ["no_kcov"],
    deps = [
        ":c_iris_collision_geometry",
        ":c_iris_test_utilities",
        "//common/test_utilities:symbolic_test_util",
    ],
)

drake_cc_googletest(
    name = "cspace_separating_plane_test",
    deps = [
        ":cspace_separating_plane",
        "//common/test_utilities:symbolic_test_util",
    ],
)

drake_cc_googletest(
    name = "geodesic_convexity_test",
    num_threads = 2,
    deps = [
        ":convex_set",
        ":geodesic_convexity",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "graph_of_convex_sets_test",
    timeout = "moderate",
    num_threads = 2,
    shard_count = 8,
    deps = [
        ":graph_of_convex_sets",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_no_throw",
        "//common/test_utilities:expect_throws_message",
        "//common/yaml",
        "//solvers:choose_best_solver",
        "//solvers:clarabel_solver",
        "//solvers:clp_solver",
        "//solvers:csdp_solver",
        "//solvers:gurobi_solver",
        "//solvers:ipopt_solver",
        "//solvers:linear_system_solver",
        "//solvers:mosek_solver",
        "//solvers:osqp_solver",
        "//solvers:scs_solver",
    ],
)

drake_cc_googletest(
    name = "implicit_graph_of_convex_sets_test",
    deps = [
        ":implicit_graph_of_convex_sets",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_no_throw",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "hpolyhedron_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:maybe_pause_for_user",
        "//common/yaml",
        "//geometry:meshcat",
        "//geometry/test_utilities:meshcat_environment",
    ],
)

drake_cc_googletest(
    name = "hyperrectangle_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//common/yaml",
    ],
)

drake_cc_googletest(
    name = "hyperellipsoid_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//common/yaml",
        "//solvers:mosek_solver",
    ],
)

drake_cc_googletest(
    name = "intersection_test",
    num_threads = 2,
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "iris_internal_test",
    deps = [
        ":iris_internal",
        "//common/test_utilities:eigen_matrix_compare",
        "//multibody/parsing:parser",
        "//solvers:ipopt_solver",
        "//systems/framework:diagram_builder",
    ],
)

drake_cc_googletest(
    name = "iris_test",
    tags = ["no_kcov"],
    deps = [
        ":iris",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//common/yaml",
    ],
)

drake_cc_googletest(
    name = "iris_np_test",
    timeout = "moderate",
    data = ["//multibody/parsing:test_models"],
    shard_count = 8,
    # Most of these tests take an exceptionally long time under
    # instrumentation, resulting in timeouts, and so are excluded.
    tags = [
        "no_asan",
        "no_memcheck",
    ],
    deps = [
        ":iris",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//common/test_utilities:maybe_pause_for_user",
        "//geometry:meshcat",
        "//geometry/test_utilities:meshcat_environment",
        "//multibody/inverse_kinematics",
        "//multibody/parsing:parser",
        "//systems/framework:diagram_builder",
    ],
)

drake_cc_googletest(
    name = "minkowski_sum_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "point_test",
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

drake_cc_googletest(
    name = "spectrahedron_test",
    num_threads = 2,
    deps = [
        ":convex_set",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
        "//solvers:solve",
    ],
)

drake_cc_googletest(
    name = "vpolytope_test",
    data = ["//geometry:test_obj_files"],
    deps = [
        ":convex_set",
        ":test_utilities",
        "//common/test_utilities:eigen_matrix_compare",
        "//common/test_utilities:expect_throws_message",
    ],
)

add_lint_tests()
